/*
 * Copyright (c) 2007-2009 Sun Microsystems, Inc.
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to 
 * deal in the Software without restriction, including without limitation the 
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 
 * sell copies of the Software, and to permit persons to whom the Software is 
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in 
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 
 * DEALINGS IN THE SOFTWARE.
 **/

package org.sunspotworld.demo;


import com.sun.spot.resources.transducers.ISwitch;
import com.sun.spot.resources.transducers.ITriColorLED;
import com.sun.spot.resources.transducers.LEDColor;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.peripheral.LIS3L02AQAccelerometer;
import com.sun.spot.util.BootloaderListener;
import com.sun.spot.util.Properties;
import com.sun.spot.util.Utils;
import java.io.IOException;
import javax.microedition.midlet.MIDlet;
import javax.microedition.midlet.MIDletStateChangeException;

/**
 * Program to manually calibrate the accelerometer on the SPOT eDemo sensor board.
 * The SPOT needs to be rotated to 6 different positions, two for each axis, and
 * button1 must be pressed when it is in each one.
 *
 * The LEDs show red for each axis that still needs to be calibrated, and green
 * for those axis directions that have already been measured. When the SPOT is
 * rotated to be aligned to an unmeasured orientation the LED for that axis will
 * be displayed as white.
 *
 * The calibration process is done twice. The second time uses the values from
 * the first to tighten the constraints on what is an acceptable orientation.
 *
 * After the calibration process the accelerometer performs a self test. If it 
 * passes then the calibration values, will be stored as persistent properties
 * in the EDemoBoard Flash memory:
 *     LIS3L02AQAccelerometer.ACCELEROMETER_ZERO_OFFSETS_PROPERTY and
 *     LIS3L02AQAccelerometer.ACCELEROMETER_GAINS_PROPERTY
 * that are read in when the SPOT is started up.
 *
 * @author Ron Goldman
 */
public class CalibrateAccelerometer extends MIDlet {

    private ISwitch sw1 = EDemoBoard.getInstance().getSwitches()[EDemoBoard.SW1];
    private ITriColorLED [] leds = EDemoBoard.getInstance().getLEDs();
    private LIS3L02AQAccelerometer acc = (LIS3L02AQAccelerometer)EDemoBoard.getInstance().getAccelerometer();

    private int xMin[] = new int[12];
    private int xMax[] = new int[12];
    private int xAve[] = new int[12];
    private int yMin[] = new int[12];
    private int yMax[] = new int[12];
    private int yAve[] = new int[12];
    private int zMin[] = new int[12];
    private int zMax[] = new int[12];
    private int zAve[] = new int[12];
    
    private int zeroX = 466;        // initial default zero offset value for uncalibrated accelerometer
    private int zeroY = 466;
    private int zeroZ = 466;
    
    /**
     * Set LEDs to show which axis have been calibrated.
     */
    private int setLeds() {
        int result = 0;
        for (int i = 0; i < 6; i++) {
            if (xMax[i] == 0) {
                leds[i].setRGB(150, 0, 0);
            } else {
                leds[i].setRGB(0, 150, 0);
                result++;
            }
            leds[i].setOn();
        }
        leds[6].setOff();
        leds[7].setOff();
        return result;
    }
    
    /**
     * Set all LEDs to specified color.
     *
     * @param color color to set LEDs
     */
    private void setAllLeds(LEDColor color) {
        for (int i = 0; i < 8; i++) {
            leds[i].setColor(color);
            leds[i].setOn();
        }
    }

    /**
     * Turn off all LEDs.
     */
    private void clearLeds() {
        for (int i = 0; i < 8; i++) {
            leds[i].setOff();
        }
    }
    
    /**
     * Take readings of accelerometer in current orientation.
     *
     * @return array of min, max, and average values for each axis.
     */
    private int[] reallyGetValues() {
        int results[] = new int[9];                     // min, max, ave
        results[0] = results[1] = results[2] = 999;     // min x, y & z
        try {
            for (int i = 0; i < 50; i++) {
                int val = acc.getRawX();
                if (results[0] > val) { results[0] = val; }     // update xMin
                if (results[3] < val) { results[3] = val; }     // update xMax
                results[6] += val;                              // update xAve
                val = acc.getRawY();
                if (results[1] > val) { results[1] = val; }     // update yMin
                if (results[4] < val) { results[4] = val; }     // update yMax
                results[7] += val;                              // update yAve
                val = acc.getRawZ();
                if (results[2] > val) { results[2] = val; }     // update zMin
                if (results[5] < val) { results[5] = val; }     // update zMax
                results[8] += val;                              // update zAve
                Utils.sleep(6);                                 // no sense sampling faster than 160hz
            }
            results[6] /= 50;                                   // compute xAve
            results[7] /= 50;                                   // compute yAve
            results[8] /= 50;                                   // compute zAve
        } catch (IOException ex) {
            System.out.println("IO Exception during testing: " + ex);
        }
        return results;
    }

    /**
     * Copy measurements for this orientation.
     *
     * @param i orientation being measured
     * @param val2G measurements using accelerometer 2G scale
     * @param val6G measurements using accelerometer 6G scale
     */
    private void copy(int i, int val2G[], int val6G[]) {
        xMin[i] = val2G[0];
        xMax[i] = val2G[3];
        xAve[i] = val2G[6];
        yMin[i] = val2G[1];
        yMax[i] = val2G[4];
        yAve[i] = val2G[7];
        zMin[i] = val2G[2];
        zMax[i] = val2G[5];
        zAve[i] = val2G[8];

        xMin[i+6] = val6G[0];
        xMax[i+6] = val6G[3];
        xAve[i+6] = val6G[6];
        yMin[i+6] = val6G[1];
        yMax[i+6] = val6G[4];
        yAve[i+6] = val6G[7];
        zMin[i+6] = val6G[2];
        zMax[i+6] = val6G[5];
        zAve[i+6] = val6G[8];

        int j = i + 6;
        System.out.println(" [ (" + xMin[i] + ", " + xAve[i] + ", " + xMax[i] + ") (" +
                                    yMin[i] + ", " + yAve[i] + ", " + yMax[i] + ") (" +
                                    zMin[i] + ", " + zAve[i] + ", " + zMax[i] + ") ]  [ (" +
                                    xMin[j] + ", " + xAve[j] + ", " + xMax[j] + ") (" +
                                    yMin[j] + ", " + yAve[j] + ", " + yMax[j] + ") (" +
                                    zMin[j] + ", " + zAve[j] + ", " + zMax[j] + ") ]\n");
    }

    /**
     * Maximum deviation in raw accelerometer reading from zero orientation.
     *
     * @param tight tolerance allowed for valid orientation
     * @return allowed tolerance
     */
    private int getOffZeroTolerance(boolean tight) {
        return tight ? 20 : 40;
    }

    /**
     * Minimal allowable raw accelerometer reading for up/down orientation.
     *
     * @param tight tolerance allowed for valid orientation
     * @return allowed tolerance
     */
    private int getAxialTolerance(boolean tight) {
        return tight ? 150 : 130;
    }
    
    /**
     * Read accelerometer in current orientation and record if valid.
     *
     * @param tight tolerance allowed for valid orientation
     */
    private void getValues(boolean tight) throws IOException {
        int offZeroTolerance = getOffZeroTolerance(tight);
        int axialTolerance = getAxialTolerance(tight);
        int val2G[] = reallyGetValues();
            
        acc.setScale(LIS3L02AQAccelerometer.SCALE_6G);    // set to 6G scale
        Utils.sleep(150);
        int val6G[] = reallyGetValues();

        acc.setScale(LIS3L02AQAccelerometer.SCALE_2G);    // set back to 2G scale
        Utils.sleep(150);

        // Decide which orientation the SPOT is in
        int b = 0;
        if (Math.abs(val2G[6] - zeroX) > offZeroTolerance) { b++; };
        if (Math.abs(val2G[7] - zeroY) > offZeroTolerance) { b++; };
        if (Math.abs(val2G[8] - zeroZ) > offZeroTolerance) { b++; };
        if (b != 1) {
            System.out.println("Not a clean orientation: " + val2G[6] + ", " + val2G[7] + ", " + val2G[8]);
        } else if (Math.abs(val2G[6] - zeroX) > axialTolerance) {        // x-axis up/down
            if (val2G[6] > zeroX) {
                copy(0, val2G, val6G);
            } else {
                copy(1, val2G, val6G);
            }
        } else if (Math.abs(val2G[7] - zeroY) > axialTolerance) {        // y-axis up/down
            if (val2G[7] > zeroY) {
                copy(2, val2G, val6G);
            } else {
                copy(3, val2G, val6G);
            }
        } else if (Math.abs(val2G[8] - zeroZ) > axialTolerance) {        // z-axis up/down
            if (val2G[8] > zeroZ) {
                copy(4, val2G, val6G);
            } else {
                copy(5, val2G, val6G);
            }
        } else {
            System.out.println("Not a clean orientation: " + val2G[6] + ", " + val2G[7] + ", " + val2G[8]);
        }
    }

    /**
     * Set LED for given axis to white to indicate valid orientation.
     * If axis has already been calibrated then don't bother.
     *
     * @param i specified orientation
     */
    private void maybeAxis(int i){
        if (xMax[i] == 0) {
            leds[i].setRGB(255, 255, 255);
        }
    }

    /**
     * Show in LEDs when SPOT is in a valid orientation.
     *
     * @param tight tolerance allowed for valid orientation
     */
    private void checkOrientation(boolean tight) {
        try {
            int offZeroTolerance = getOffZeroTolerance(tight);
            int axialTolerance = getAxialTolerance(tight);
            int x = acc.getRawX();
            int y = acc.getRawY();
            int z = acc.getRawZ();
            // Decide which orientation the SPOT is in
            int b = 0;
            if (Math.abs(x - zeroX) > offZeroTolerance) { b++; };
            if (Math.abs(y - zeroY) > offZeroTolerance) { b++; };
            if (Math.abs(z - zeroZ) > offZeroTolerance) { b++; };
            if (b == 1) {
                if (Math.abs(x - zeroX) > axialTolerance) {               // x-axis up/down
                    if (x > zeroX) {
                        maybeAxis(0);
                    } else {
                        maybeAxis(1);
                    }
                } else if (Math.abs(y - zeroY) > axialTolerance) {        // y-axis up/down
                    if (y > zeroY) {
                        maybeAxis(2);
                    } else {
                        maybeAxis(3);
                    }
                } else if (Math.abs(z - zeroZ) > axialTolerance) {        // z-axis up/down
                    if (z > zeroZ) {
                        maybeAxis(4);
                    } else {
                        maybeAxis(5);
                    }
                } else {
                    setLeds();
                }
            } else {
                setLeds();
            }
        } catch (IOException ex) {
            // ignore
        }
    
    }
    
    /**
     * Wait for the state of specified switch to change. While waiting show when
     * in a valid orientation in LEDs.
     *
     * @param sw switch to monitor
     * @param tight tolerance allowed for valid orientation
     */
    private void myWaitForChange(ISwitch sw, boolean tight) {
        boolean startState = sw.isOpen();
        while(sw.isOpen() == startState) {
            try {
                checkOrientation(tight);
                Thread.sleep(75);
            } catch (InterruptedException e) {
                System.out.println("Sleep interrupted!");
            }
        }
    }

    /**
     * Measure all 6 orientations and compute calibration values.
     *
     * @param tight specify tolerance of orientation: loose = false, tight = true
     */
    private void calibrate(boolean tight) throws IOException {
        for (int i = 0; i < 12; i++) {      // Zero out calibration values
            xMin[i] = xMax[i] = xAve[i] = 0;
            yMin[i] = yMax[i] = yAve[i] = 0;
            zMin[i] = zMax[i] = zAve[i] = 0;
        }

        while (setLeds() < 6) {             // Measure each axis, both orientations
            leds[7].setColor(LEDColor.RED);
            leds[7].setOn();
            if (sw1.isOpen()) { myWaitForChange(sw1, tight); }  // wait for button press
            while (sw1.isClosed()) { myWaitForChange(sw1, tight); }
            clearLeds();
            for (int i = 7; i >= 0; i--) {          // display countdown to measuring
                leds[i].setColor(LEDColor.GREEN);
                leds[i].setOn();
                Utils.sleep(125);
                leds[i].setOff();
            }
            getValues(tight);                       // measure this orientation
        }
        
        System.out.println("\nCalibration values:");
        for (int i = 0; i < 6; i++) {
            int j = i + 6;
            System.out.println(" [ (" + xMin[i] + ", " + xAve[i] + ", " + xMax[i] + ") (" +
                                        yMin[i] + ", " + yAve[i] + ", " + yMax[i] + ") (" +
                                        zMin[i] + ", " + zAve[i] + ", " + zMax[i] + ") ]  [ (" +
                                        xMin[j] + ", " + xAve[j] + ", " + xMax[j] + ") (" +
                                        yMin[j] + ", " + yAve[j] + ", " + yMax[j] + ") (" +
                                        zMin[j] + ", " + zAve[j] + ", " + zMax[j] + ") ]");
        }
        double zeroX2G = (xAve[2] + xAve[3] + xAve[4] + xAve[5]) / 4.0;
        double zeroY2G = (yAve[0] + yAve[1] + yAve[4] + yAve[5]) / 4.0;
        double zeroZ2G = (zAve[0] + zAve[1] + zAve[2] + zAve[3]) / 4.0;
        double zeroX6G = (xAve[8] + xAve[9] + xAve[10] + xAve[11]) / 4.0;
        double zeroY6G = (yAve[6] + yAve[7] + yAve[10] + yAve[11]) / 4.0;
        double zeroZ6G = (zAve[6] + zAve[7] + zAve[8] + zAve[9]) / 4.0;
        System.out.println("\nAverage zero values: (" + zeroX2G + ", " + zeroY2G + ", "+ zeroZ2G + ")  (" +
                                                        zeroX6G + ", " + zeroY6G + ", "+ zeroZ6G + ")");
        zeroX = (int)zeroX2G;
        zeroY = (int)zeroY2G;
        zeroZ = (int)zeroZ2G;

        double x2G = (xAve[0] - xAve[1]) / 2.0;
        double y2G = (yAve[2] - yAve[3]) / 2.0;
        double z2G = (zAve[4] - zAve[5]) / 2.0;
        double x6G = (xAve[6] - xAve[7]) / 2.0;
        double y6G = (yAve[8] - yAve[9]) / 2.0;
        double z6G = (zAve[10] - zAve[11]) / 2.0;
        System.out.println("\nSensitivity values: (" + x2G + ", " + y2G + ", "+ z2G + ")  (" +
                                                       x6G + ", " + y6G + ", "+ z6G + ")\n");
    }

    private void printVal(int vals[]) {
        System.out.print("(" + vals[0] + ", " + vals[6] + ", " + vals[3] + ") (" +
                               vals[1] + ", " + vals[7] + ", " + vals[4] + ") (" +
                               vals[2] + ", " + vals[8] + ", " + vals[5] + ")");
    }

    /**
     * Do self-test to check that accelerometer is working properly.
     *
     * @return true if self-test passed, false if no good.
     */
    private boolean selftest() throws IOException {
        setAllLeds(LEDColor.BLUE);
        while (sw1.isOpen()) { sw1.waitForChange(); }
        while (sw1.isClosed()) { sw1.waitForChange(); }
        clearLeds();
        for (int i = 7; i >= 0; i--) {
            leds[i].setColor(LEDColor.BLUE);
            leds[i].setOn();
            Utils.sleep(125);
            leds[i].setOff();
        }
        int val2G[] = reallyGetValues();

        acc.setScale(LIS3L02AQAccelerometer.SCALE_6G);    // set to 6G scale
        Utils.sleep(150);
        int val6G[] = reallyGetValues();

        acc.setScale(LIS3L02AQAccelerometer.SCALE_2G);    // set back to 2G scale
        Utils.sleep(150);
        acc.selfTest(true); // turn on self-test mode
        Utils.sleep(200);
        int tval2G[] = reallyGetValues();

        acc.setScale(LIS3L02AQAccelerometer.SCALE_6G);    // set to 6G scale
        Utils.sleep(150);
        int tval6G[] = reallyGetValues();

        acc.selfTest(false); // disable self-test mode
        acc.setScale(LIS3L02AQAccelerometer.SCALE_2G);    // set back to 2G scale

        System.out.print("\nRegular values: \n [ ");
        printVal(val2G);
        System.out.print(" ]  [ ");
        printVal(val6G);
        System.out.println(" ]");

        System.out.print("Self-test values: \n [ ");
        printVal(tval2G);
        System.out.print(" ]  [ ");
        printVal(tval6G);
        System.out.println(" ]");

        boolean result = true;
        int dX2 = tval2G[6] - val2G[6];
        int dY2 = tval2G[7] - val2G[7];
        int dZ2 = tval2G[8] - val2G[8];
        int dX6 = tval6G[6] - val6G[6];
        int dY6 = tval6G[7] - val6G[7];
        int dZ6 = tval6G[8] - val6G[8];
        System.out.println("Differences: [ (" + dX2 + ", " + dY2 + ", " + dZ2 + ") (" + 
                                                dX6 + ", " + dY6 + ", " + dZ6 + ") ]");
        if (dX2 < -13 || -6 < dX2) {
            System.out.println("X-axis failed self-test at 2G: " + dX2 + " not in range (-13, -6)");
            result = false;
        }
        if (dY2 < 6 || 13 < dY2) {
            System.out.println("Y-axis failed self-test at 2G: " + dY2 + " not in range (6, 13)");
            result = false;
        }
        if (dZ2 < 6 || 16 < dZ2) {
            System.out.println("Z-axis failed self-test at 2G: " + dZ2 + " not in range (6, 16)");
            result = false;
        }

        if (dX6 < -5 || -1 < dX6) {
            System.out.println("X-axis failed self-test at 6G: " + dX6 + " not in range (-5, -1)");
            result = false;
        }
        if (dY6 < 1 || 5 < dY6) {
            System.out.println("Y-axis failed self-test at 6G: " + dY6 + " not in range (1, 5)");
            result = false;
        }
        if (dZ6 < -1 || 6 < dZ6) {
            System.out.println("Z-axis failed self-test at 6G: " + dZ6 + " not in range (1, 6)");
            result = false;
        }
        return result;
    }

    /**
     * Save the calibration values as persistent properties in the EDemoBoard
     * Flash memory.
     */
    private void saveCalibration() {
        double[][] offs = new double[2][3];
        offs[0][0] = (xAve[2] + xAve[3] + xAve[4] + xAve[5]) / 4.0;
        offs[0][1] = (yAve[0] + yAve[1] + yAve[4] + yAve[5]) / 4.0;
        offs[0][2] = (zAve[0] + zAve[1] + zAve[2] + zAve[3]) / 4.0;
        offs[1][0] = (xAve[8] + xAve[9] + xAve[10] + xAve[11]) / 4.0;
        offs[1][1] = (yAve[6] + yAve[7] + yAve[10] + yAve[11]) / 4.0;
        offs[1][2] = (zAve[6] + zAve[7] + zAve[8] + zAve[9]) / 4.0;
        acc.setZeroOffsets(offs);

        double[][] gains = new double[2][3];
        gains[0][0] = (xAve[0]  - xAve[1]) / 2.0;
        gains[0][1] = (yAve[2]  - yAve[3]) / 2.0;
        gains[0][2] = (zAve[4]  - zAve[5]) / 2.0;
        gains[1][0] = (xAve[6]  - xAve[7]) / 2.0;
        gains[1][1] = (yAve[8]  - yAve[9]) / 2.0;
        gains[1][2] = (zAve[10] - zAve[11]) / 2.0;
        acc.setGains(gains);
        acc.saveCalibration();
    }

    /**
     * See if accelerometer has already been calibrated.
     */
    private void getCalibration() {
        Properties prop = EDemoBoard.getInstance().getProperties();
        String offsets = prop.getProperty(LIS3L02AQAccelerometer.ACCELEROMETER_ZERO_OFFSETS_PROPERTY);
        String gains   = prop.getProperty(LIS3L02AQAccelerometer.ACCELEROMETER_GAINS_PROPERTY);
        if (offsets != null) {
            System.out.println("\nStored accelerometer calibration values:");
            System.out.println("   offsets = " + offsets);
            System.out.println("   gains = " + gains);
        } else {
            System.out.println("\nAccelerometer not calibrated yet");
        }
    }
    
    /**
     * Calibrate the accelerometer.
     */
    private void checkoutAccelerometer() throws IOException {
        getCalibration();
        System.out.println("\nCalibrate accelerometer please:");
        calibrate(false);
        System.out.println("\nRecalibrate please:");
        calibrate(true);
        System.out.println("\nReady to do self test:");
        if (selftest()) {
            System.out.println("\nSaving accelerometer calibration");
            saveCalibration();                 // Only saved if passes self-test
            setAllLeds(LEDColor.GREEN);
        } else {
            setAllLeds(LEDColor.RED);
        }
    }

    protected void startApp() throws MIDletStateChangeException {
        new BootloaderListener().start();       // Listen for downloads/commands over USB connection
        try {
            checkoutAccelerometer();
        } catch (IOException ex) {
            System.out.println("\nError calibrating accelerometer: " + ex);
        }
        clearLeds();
        notifyDestroyed();          // all done here
    }

    protected void pauseApp() {
        // This will never be called by the Squawk VM
    }

    /**
     * Called if the MIDlet is terminated by the system.
     * I.e. if startApp throws any exception other than MIDletStateChangeException,
     * if the isolate running the MIDlet is killed with Isolate.exit(), or
     * if VM.stopVM() is called.
     * 
     * It is not called if MIDlet.notifyDestroyed() was called.
     */
    protected void destroyApp(boolean arg0) throws MIDletStateChangeException {
        
    }
}
